#include "task.h"



void delay_ms()
{
    delay_cycles(32000000 / 1000); // 1ms
}

void User_delay(int time)
{
    for (int i = 0; i < time; i++)
    {
        delay_ms();
    }
}

void stop()
{
    // 停
    Motor_Driver(&motor[0], 0, 0);
    Motor_Driver(&motor[1], 0, 0);
    User_delay(500);
}

void turning(int angle_set)
{
    stop();
    Servo_Driver(&turnover, angle_set);
    User_delay(1000);
}

void go(int time)
{
    stop();
    Motor_Driver(&motor[0], 800, 1);
    Motor_Driver(&motor[1], 800, 1);
    User_delay(time);
}

void back(int time)
{
    stop();
    Motor_Driver(&motor[0], 800, 0);
    Motor_Driver(&motor[1], 800, 0);
    User_delay(time);
}

void daoku()
{
    // 舵机转向
    turning(110);
    // 倒车
    back(1300);
    turning(reference_angle);
    back(800);
    turning(reference_angle);
    stop();
    User_delay(2000);
    go(800);
    turning(110);
    go(1300);
    turning(reference_angle);
    stop();
}

void cefang()
{
    turning(100);
    back(1250);
    turning(40);
    back(1100);
    turning(reference_angle);
    stop();
    User_delay(2000);
    turning(30);
    go(1100);
    turning(100);
    go(1250);
    turning(reference_angle);
    stop();
}
